//
// Created by clarence on 2021/6/17.
//

#include <ros/ros.h>

#include <tf/transform_datatypes.h>

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/time_synchronizer.h>

#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/Point32.h>
#include <std_msgs/Float64MultiArray.h>
#include <sensor_msgs/PointCloud2.h>
#include <visualization_msgs/Marker.h>
#include <pcl/common/transforms.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <iostream>
#include <string>
#include <cmath>
#include <algorithm>
#include <tf/transform_broadcaster.h>
#include <time.h>
#include <stdlib.h>
#include <mavros_msgs/State.h>
#include <random>
#include <nav_msgs/Odometry.h>
#include <control_msgs/JointControllerState.h>
#include <std_msgs/Float64.h>
#include <queue>
#include <vector>
#include <fstream>
#include <cstdint>
#include <trajectory_msgs/JointTrajectoryPoint.h>
#include "dsp-nonegaussian-dst.h"
#include "gazebo_msgs/ModelStates.h"
#include "gazebo_msgs/ModelState.h"
#include "mavros_msgs/State.h"
#include "line_segment_distance.h"

using namespace message_filters;
using namespace std;

#define PIx2 6.28318
#define PI 3.14159
#define PI_2 1.5708


/**** Parameters to tune, some initialization needs to be changed in main function ****/
VideMap my_map;

const unsigned int MAX_POINT_NUM  = 5000;
float point_clouds[MAX_POINT_NUM*4];

float x_min = -MAP_LENGTH_VOXEL_NUM * VOXEL_RESOLUTION / 2;
float x_max = MAP_LENGTH_VOXEL_NUM * VOXEL_RESOLUTION / 2;
float y_min = -MAP_WIDTH_VOXEL_NUM * VOXEL_RESOLUTION / 2;
float y_max = MAP_WIDTH_VOXEL_NUM * VOXEL_RESOLUTION / 2;
float z_min = -MAP_HEIGHT_VOXEL_NUM * VOXEL_RESOLUTION / 2;
float z_max = MAP_HEIGHT_VOXEL_NUM * VOXEL_RESOLUTION / 2;
float res = 0.15;  // Smaller res will get better tracking result but is slow.


Eigen::Vector3d map_center_point;

Eigen::Vector3d last_send_p, last_send_v, last_send_a;

const float CAL_DURATION = 0.050f; // 20Hz

const float RISK_MIN_THRESHOLD = 0.05f; //RISK lower than this value will be treated as zero
const float RISK2OCCUPIED_THRESHOLD = 0.3f; //RISK higher than this value will be treated as occupiedc
const float LOW_RISK_THRESHOLD = 10.f;

const int ANGLE_H_NUM_SPARSE = 11;
const int ANGLE_V_NUM_SPARSE = 5;
const int ANGLE_H_NUM_DENSE = 21;
const int ANGLE_V_NUM_DENSE = 13;

float MAX_V_XY = 1.f;
float MAX_V_Z_UP = 1.f;
float MAX_V_Z_DOWN = 0.5f;
float MAX_A = 1.5f;
float HEIGHT_LIMIT = 2.f;
float XY_LIMIT = 2.5f;

float ABSOLUTE_SAFE_TIME_PIECE1 = 1.0f;
float ABSOLUTE_SAFE_TIME_PIECE2 = 1.0f;

float MAX_RISK_ONE_PRIMITIVE = 1000.0;

float DIRECTION_CHANGE_LEFT_SIZE = 1.5;

float PLAN_INIT_STATE_CHANGE_THRESHOLD = 0.1;

bool DIRECTION_AUTO_CHANGE = false;

bool map_status_locked = false;
bool state_locked = false;
bool offboard_ready = false;
bool planning_finished = false;

bool send_global_path = false;
int global_path_sending_seq = 0;

int traj1_points_num_to_keep_global = 100;

std::string TABLE_PATH, GOLBAL_TRAJECTORY_PATH;


struct  Path_Planning_Parameters
{
    float d_ref = 1.6;  //NOTE: NEED TO CHANGE CHG
    float k1_xy = 5; //% Goal directed coefficient
    float k1_z = 3; //% Goal directed coefficient
    float k2_xy = 1; //% Rotation coefficient 3
    float k2_z = 1.5; //% Rotation coefficient 3.5
    float k_near_global_traj = 3.0;
    float k_long_dist_ref = 3.0;
    float v_max_at_goal_ori = 0.3; //% m/s, just reference  5.0 originally
    float v_scale_min = 0.1;
    float collision_threshold_static = 0.6;
    float collision_threshold_dynamic = 1.0;

    float dynamic_future_risk_threshold = 5.0;
    float connect_global_path_threshold = 0.5;
    float risk_checking_radius = 1.2;
}pp;


float MAP_DELAY_SECONDS = 0.05;
const int point_num_pub = 5; // For the visualization of current planned trajectory

bool safe_trajectory_avaliable = true;
bool use_position_global_time = false;
bool if_plan_vertical_path = true;
bool if_flyback_enable = true;
bool if_in_simulation = true;
bool trajectory_initialized = false;
bool trajectory_planned = false;
bool current_pose_received = false;
bool if_use_fence = false;


std::queue<double> pose_timestamp_queue;
std::queue<Eigen::Quaternionf> att_queue;
std::queue<Eigen::Vector3d> pos_queue;


/****** Global variables for path planning ******/
Eigen::VectorXf Fov_half(2); //Fov parameters
Eigen::VectorXf Angle_h_sparse(ANGLE_H_NUM_SPARSE);  // initiate later in the main function, rad
Eigen::VectorXf Angle_v_sparse(ANGLE_V_NUM_SPARSE); // initiate later in the main function, rad
Eigen::VectorXf Angle_h_dense(ANGLE_H_NUM_DENSE);  // initiate later in the main function, rad
Eigen::VectorXf Angle_v_dense(ANGLE_V_NUM_DENSE); // initiate later in the main function, rad

Eigen::Vector3d p_goal;
Eigen::Vector3d p_goal_raw;
Eigen::Vector3d p0;
Eigen::Vector3d v0;
Eigen::Vector3d a0;
Eigen::Quaternionf quad(1.0, 0.0, 0.0, 0.0);
Eigen::Vector3d init_p;
float yaw0;
float yaw0_rate;

bool global_trajectory_imported = false;

vector<Eigen::Vector3d> global_traj_p, global_traj_v, global_traj_a;
vector<Eigen::Vector3d> global_traj_key_points;
vector<int> global_traj_key_points_seq;
vector<Eigen::Vector3d> traj_p_buffer_1, traj_p_buffer_2;
vector<Eigen::Vector3d> traj_v_buffer_1, traj_v_buffer_2;
vector<Eigen::Vector3d> traj_a_buffer_1, traj_a_buffer_2;
Eigen::MatrixXd traj_p_buffer_2_matrix;

double yaw_sp = 0.f;
double yaw_sp_last = 0.f;

bool trajectory_buffer_1_requires_update = true;
bool trajectory_buffer_2_requires_update = true;
int trajectory_1_send_counter = 0;


struct ActorState{
    string name;
    double record_time = 0.0;
    double px = 0.0;
    double py = 0.0;
    double pz = 0.0;
    double yaw = 0.0;
    double vx = 0.0;
    double vy = 0.0;
    double vz = 0.0;
};

struct StaticObstacleState{
    string name;
    double record_time;
    double px = 0.f;
    double py = 0.f;
    double pz = 0.f;
    double radius = 0.5;
};

vector<ActorState> actor_avg_states;
vector<StaticObstacleState> static_obstacles_vector;

ros::Publisher pva_pub, current_marker_pub;

void setParameters();

void marker_publish(const Eigen::MatrixXd &Points, int id, float r, float g, float b, float width, int publish_num, int type=4);

void marker_publish(const vector<Eigen::Vector3d> &Points, int id, float r, float g, float b, float width, int publish_num, int type=4);

void textPublish(string str, int id, float px, float py, float pz);

/**********************FOR HSH TABLE***************************/
// PVA table for feasible total planning time T
typedef struct pva_table pva_table;
struct pva_table {
    int dim_num;
    int *dim_size;
    uint64_t *dim_interval;
    uint64_t table_size;
    float *table;
    float rez;
    float *pva_limit;

    void construct_pva_table(int dim1_size, int dim2_size, int dim3_size, int dim4_size, float resolution) {
        this->dim_num = 4;
        this->dim_size = (int*)malloc(sizeof(int)*this->dim_num);
        this->dim_interval = (uint64_t*)malloc(sizeof(uint64_t)*this->dim_num);

        this->dim_size[0] = dim1_size;
        this->dim_size[1] = dim2_size;
        this->dim_size[2] = dim3_size;
        this->dim_size[3] = dim4_size;

        this->dim_interval[3] = 1;
        this->dim_interval[2] = dim_interval[3] * dim4_size;
        this->dim_interval[1] = dim_interval[2] * dim3_size;
        this->dim_interval[0] = dim_interval[1] * dim2_size;

        this->table_size = this->dim_interval[0] * dim1_size;
        this->table = (float*)malloc(sizeof(float)*this->table_size);

        this->rez = resolution;

        this->pva_limit = (float*)malloc(sizeof(float)*3);
        this->pva_limit[0] = this->rez*float(dim1_size/2);
        this->pva_limit[1] = this->rez*float(dim2_size/2);
        this->pva_limit[2] = this->rez*float(dim4_size/2);
    }

    void compute_idx_from_pva(float dlt_p, float v0, float vf, float a0,
                              int &idx1, int &idx2, int &idx3, int &idx4) {
        idx1 = round(dlt_p/this->rez) + this->dim_size[0]/2;
        idx2 = round(v0/this->rez) + this->dim_size[1]/2;
        idx3 = round(vf/this->rez) + this->dim_size[2]/2;
        idx4 = round(a0/this->rez) + this->dim_size[3]/2;
    }

    float query_pva_table(float dlt_p, float v0, float vf, float a0) {
        if (fabs(dlt_p) > this->pva_limit[0]) static_assert("Invalid input!", "");
        if (fabs(v0) > this->pva_limit[1]) static_assert("Invalid input!", "");
        if (fabs(vf) > this->pva_limit[1]) static_assert("Invalid input!", "");
        if (fabs(a0) > this->pva_limit[2]) static_assert("Invalid input!", "");

        int idx1, idx2, idx3, idx4;
        this->compute_idx_from_pva(dlt_p, v0, vf, a0, idx1, idx2, idx3, idx4);

        uint64_t idx = idx1*this->dim_interval[0] + idx2*this->dim_interval[1] +
                       idx3*this->dim_interval[2] + idx4*this->dim_interval[3];

        // std::cout << "idx: " << idx << std::endl;

        return this->table[idx];
    }

    void pva_table2csv(const std::string &str) {
        std::ofstream outfile;
        outfile.open(str, std::ios::out);

        for (int i = 0; i < 4; ++i) outfile << std::to_string(this->dim_size[i]) << ',';
        outfile << std::to_string(this->rez) << std::endl;

        for (uint64_t i = 0; i < this->table_size-1; ++i) outfile << std::to_string(this->table[i]) << ',';
        outfile << std::to_string(this->table[table_size-1]);

        outfile.close();
    }

    void csv2pva_table(const std::string &str) {
        int tmp_dim_size[4];
        float tmp_rez;

        std::ifstream infile(str, std::ios::in);
        std::string tmp_str;

        for (int i = 0; i < 4; ++i) {
            getline(infile, tmp_str, ',');
            tmp_dim_size[i] = std::stoi(tmp_str);
        }

        getline(infile, tmp_str);
        tmp_rez = std::stod(tmp_str);

        this->construct_pva_table(tmp_dim_size[0], tmp_dim_size[1],
                                  tmp_dim_size[2], tmp_dim_size[3], tmp_rez);

        for (uint64_t i = 0; i < this->table_size; ++i) {
            getline(infile, tmp_str, ',');
            this->table[i] = std::stod(tmp_str);
        }
    }

    void free_pva_table() {
        free(this->pva_limit);
        free(this->table);
        free(this->dim_interval);
        free(this->dim_size);
    }
};

pva_table *trajectory_time_table = (pva_table*)malloc(sizeof(pva_table));
/***************************************************/


/** This function is to generate state to state trajectory **/
bool motion_primitives_with_table_va_free(Eigen::Vector3d p0, Eigen::Vector3d v0, Eigen::Vector3d a0,  Eigen::Vector3d pf,
                                          Eigen::Vector3d goal,float v_final, float delt_t,
                                          Eigen::MatrixXd &p, Eigen::MatrixXd &v, Eigen::MatrixXd &a, Eigen::VectorXd &t)
{
    float delt_x = pf(0) - p0(0);
    float delt_y = pf(1) - p0(1);
    float delt_z = pf(2) - p0(2);

    float delt_length = sqrt(delt_x*delt_x + delt_y*delt_y + delt_z*delt_z);
    if(delt_length > pp.d_ref){
        float scale = pp.d_ref / delt_length;
        delt_x = delt_x * scale;
        delt_y = delt_y * scale;
        delt_z = delt_z * scale;
    }

    Eigen::Vector3d l = goal - pf;
    Eigen::Vector3d vf;
    double l_norm = l.norm();
    if(fabs(l_norm) > 0.0001){
        vf = (v_final / l.norm()) * l;
        vf(2) = 0;
    }else{
        vf = Eigen::Vector3d::Zero();
    }


    Eigen::Vector3d af = Eigen::Vector3d::Zero();

    static float j_limit = 4;
    static float a_limit = MAX_A * 0.8;
    static float v_limit = MAX_V_XY * 0.8;

    for(int i=0; i<3; i++){
        if(fabs(v0(i)) > v_limit) {
            v0(i) = v_limit * v0(i) / fabs(v0(i));
            ROS_WARN_THROTTLE(1, "v_limit in local planning is large than the values in the loaded table!  v0 max=%f", v0.cwiseAbs().maxCoeff());
        }
        if(fabs(a0(i)) > a_limit) {
            a0(i) = a_limit * a0(i) / fabs(a0(i));
            ROS_WARN_THROTTLE(1, "a_limit in local planning is large than the values in the loaded table!  a0 max=%f", a0.cwiseAbs().maxCoeff());
        }
    }

    float T1, T2, T3, T;
    T1 = trajectory_time_table->query_pva_table(delt_x, v0(0), vf(0), a0(0));
    T2 = trajectory_time_table->query_pva_table(delt_y, v0(1), vf(1), a0(1));
    T3 = trajectory_time_table->query_pva_table(delt_z, v0(2), vf(2), a0(2));

    if (T1 == -1 || T2 == -1 || T3 == -1) {
        return false;
    }

    T = T1 > T2 ? T1 : T2;
    T = T > T3 ? T : T3;
    T = T < 0.5 ? 0.5 : T;

//    T *= 0.6;  //chg

    int times = T / delt_t;

    p = Eigen::MatrixXd::Zero(times, 3);
    v = Eigen::MatrixXd::Zero(times, 3);
    a = Eigen::MatrixXd::Zero(times, 3);
    t = Eigen::VectorXd::Zero(times);

    // % calculate optimal jerk controls by Mark W. Miller

    for(int ii=0; ii<3; ii++)
    {
        float delt_a = af(ii) - a0(ii);
        float delt_v = vf(ii) - v0(ii) - a0(ii)*T;
        float delt_p = pf(ii) - p0(ii) - v0(ii)*T - 0.5*a0(ii)*T*T;

        // % if vf is not free
        float alpha = delt_a*60/pow(T,3) - delt_v*360/pow(T,4) + delt_p*720/pow(T,5);
        float beta = -delt_a*24/pow(T,2) + delt_v*168/pow(T,3) - delt_p*360/pow(T,4);
        float gamma = delt_a*3/T - delt_v*24/pow(T,2) + delt_p*60/pow(T,3);

        for(int jj=0; jj<times; jj++)
        {
            float tt = (jj+1) * delt_t;
            t(jj) = tt;
            p(jj,ii) = alpha/120*pow(tt,5) + beta/24*pow(tt,4) + gamma/6*pow(tt,3) + a0(ii)/2*pow(tt,2) + v0(ii)*tt + p0(ii);
            v(jj,ii) = alpha/24*pow(tt,4) + beta/6*pow(tt,3) + gamma/2*pow(tt,2) + a0(ii)*tt + v0(ii);
            a(jj,ii) = alpha/6*pow(tt,3) + beta/2*pow(tt,2) + gamma*tt + a0(ii);
        }
    }

    return true;
}



/** This function is to generate state to state trajectory **/
bool motion_primitives_with_table_va_fixed(Eigen::Vector3d p0, Eigen::Vector3d v0, Eigen::Vector3d a0, float delt_t,
                                           Eigen::Vector3d &pf, Eigen::Vector3d &vf, Eigen::Vector3d &af,
                                           Eigen::MatrixXd &p, Eigen::MatrixXd &v, Eigen::MatrixXd &a, Eigen::VectorXd &t)
{
    float delt_x = pf(0) - p0(0);
    float delt_y = pf(1) - p0(1);
    float delt_z = pf(2) - p0(2);

    float delt_length = sqrt(delt_x*delt_x + delt_y*delt_y + delt_z*delt_z);

    if(delt_length > pp.d_ref){
        float scale = pp.d_ref / delt_length;
        delt_x = delt_x * scale;
        delt_y = delt_y * scale;
        delt_z = delt_z * scale;
    }

    static float j_limit = 4;
    static float a_limit = MAX_A * 0.8;
    static float v_limit = MAX_V_XY * 0.8;

    for(int i=0; i<3; i++){
        if(fabs(v0(i)) > v_limit) {
            v0(i) = v_limit * v0(i) / fabs(v0(i));
            ROS_WARN_THROTTLE(1, "v_limit in local planning is large than the values in the loaded table!  v0 max=%f", v0.cwiseAbs().maxCoeff());
        }
        if(fabs(a0(i)) > a_limit) {
            a0(i) = a_limit * a0(i) / fabs(a0(i));
            ROS_WARN_THROTTLE(1, "a_limit in local planning is large than the values in the loaded table!  a0 max=%f", a0.cwiseAbs().maxCoeff());
        }
    }

    float T1, T2, T3, T;
    T1 = trajectory_time_table->query_pva_table(delt_x, v0(0), vf(0), a0(0));
    T2 = trajectory_time_table->query_pva_table(delt_y, v0(1), vf(1), a0(1));
    T3 = trajectory_time_table->query_pva_table(delt_z, v0(2), vf(2), a0(2));

    if (T1 == -1 || T2 == -1 || T3 == -1) {
        return false;
    }

    T = T1 > T2 ? T1 : T2;
    T = T > T3 ? T : T3;
    T = T < 0.5 ? 0.5 : T;

//    T *= 0.6;  //chg


    int times = T / delt_t;

    p = Eigen::MatrixXd::Zero(times, 3);
    v = Eigen::MatrixXd::Zero(times, 3);
    a = Eigen::MatrixXd::Zero(times, 3);
    t = Eigen::VectorXd::Zero(times);

    // % calculate optimal jerk controls by Mark W. Miller

    for(int ii=0; ii<3; ii++)
    {
        float delt_a = af(ii) - a0(ii);
        float delt_v = vf(ii) - v0(ii) - a0(ii)*T;
        float delt_p = pf(ii) - p0(ii) - v0(ii)*T - 0.5*a0(ii)*T*T;

        // % if vf is not free
        float alpha = delt_a*60/pow(T,3) - delt_v*360/pow(T,4) + delt_p*720/pow(T,5);
        float beta = -delt_a*24/pow(T,2) + delt_v*168/pow(T,3) - delt_p*360/pow(T,4);
        float gamma = delt_a*3/T - delt_v*24/pow(T,2) + delt_p*60/pow(T,3);

        for(int jj=0; jj<times; jj++)
        {
            float tt = (jj+1) * delt_t;
            t(jj) = tt;
            p(jj,ii) = alpha/120*pow(tt,5) + beta/24*pow(tt,4) + gamma/6*pow(tt,3) + a0(ii)/2*pow(tt,2) + v0(ii)*tt + p0(ii);
            v(jj,ii) = alpha/24*pow(tt,4) + beta/6*pow(tt,3) + gamma/2*pow(tt,2) + a0(ii)*tt + v0(ii);
            a(jj,ii) = alpha/6*pow(tt,3) + beta/2*pow(tt,2) + gamma*tt + a0(ii);
        }
    }

    return true;
}


int getMapCheckSeq(double t){
    static double check_times[PREDICTION_TIMES-1];
    static bool check_initialized = false;
    if(!check_initialized){
        // Set check time
        for(int i=0; i<PREDICTION_TIMES-1; ++i){
            check_times[i] = (prediction_future_time[i] + prediction_future_time[i+1]) / 2.f;
        }
        check_initialized = true;
    }

    int risk_map_seq = PREDICTION_TIMES - 1;
    for(int i=0; i<PREDICTION_TIMES-1; ++i){
        if(t < check_times[i]){
            risk_map_seq = i;
            break;
        }
    }

    return risk_map_seq;
}



bool collisionCheckingStaticFence(const vector<Eigen::Vector3d> &p_vector){
    if(!if_use_fence){
        return true;
    }

    /// Fence
    for(auto &p : p_vector){
        if(p.x() < -1.f && fabs(p.y()) > 0.8f){
            return false;
        }
//        else if(p.x() < -0.5f && fabs(p.y() > 1.f)){
//            return false;
//        }
        else if(p.x() < 1.0f && p.y() > 0.3f){
            return false;
        }
        else if(fabs(p.y()) > 2.2f){
            return false;
        }
        else if(fabs(p.x()) > 5.5f){
            return  false;
        }
    }
    return true;
}

bool collisionCheckingStaticObstacles(const vector<Eigen::Vector3d> &p_vector, const Eigen::Vector3d &start_p, double obstacle_range_limit = 5.0)
{
    vector<StaticObstacleState> close_obstacles;
    if(static_obstacles_vector.empty()) return true;

    static const double obstacle_range_limit_square =  obstacle_range_limit*obstacle_range_limit;

    for(auto &ob : static_obstacles_vector){
        if( (ob.px-start_p.x())*(ob.px-start_p.x()) + (ob.py-start_p.y())*(ob.py-start_p.y()) < obstacle_range_limit_square ){
            close_obstacles.emplace_back(ob);
        }
    }

    static const double static_ob_dist_threshold_square = pp.collision_threshold_static * pp.collision_threshold_static;
    for(auto &p : p_vector){
        for(auto &ob : close_obstacles){
            double distance_2d_square = (p.x()-ob.px)*(p.x()-ob.px) + (p.y()-ob.py)*(p.y()-ob.py);
            if(distance_2d_square < static_ob_dist_threshold_square + ob.radius * ob.radius){  //cylinder model
                return false;
            }
        }
    }

    return true;
}


bool collisionCheckingDynamic(const vector<Eigen::Vector3d> &p_left, const Eigen::Vector3d &start_p, double start_time = 0.0, double dynamic_obstacle_range_limit = 5.f){
    // Filter out close obstacles
    vector<ActorState> close_actors;
    for(auto &actor : actor_avg_states){
        /// TODO: USE 3D distance
        double distance_2d = sqrt((start_p(0) - actor.px)*(start_p(0) - actor.px) +
                                  (start_p(1) - actor.py)*(start_p(1) - actor.py));
        if(distance_2d <= dynamic_obstacle_range_limit){
            close_actors.emplace_back(actor);
        }
    }

    // Check collision
    for(int i=0; i < p_left.size(); ++i){
        double check_t = i * CAL_DURATION + start_time;
        /// TODO: USE 3D distance
        double check_px = p_left[i](0);
        double check_py = p_left[i](1);
        double check_pz = p_left[i](2);

        if(check_pz > HEIGHT_LIMIT){
            return false;
        }

        for(auto const &actor : close_actors){
            double actor_px_t = actor.px + actor.vx * check_t;
            double actor_py_t = actor.py + actor.vy * check_t;

            double check_distance_2d_square = (check_px-actor_px_t)*(check_px-actor_px_t) + (check_py-actor_py_t)*(check_py-actor_py_t);
            if(check_distance_2d_square  < pp.collision_threshold_dynamic*pp.collision_threshold_dynamic){
                return false;
            }
        }

    }

    return true;
}


bool collisionCheckingWithoutRisk(const vector<Eigen::Vector3d> &p_left, const Eigen::Vector3d &start_p, double start_time = 0.0, double dynamic_obstacle_range_limit = 5.f){
    if(!collisionCheckingStaticFence(p_left)){
        return false;
    }

    if(!collisionCheckingStaticObstacles(p_left, start_p)){
        return false;
    }

    if(!collisionCheckingDynamic(p_left, start_p, start_time, dynamic_obstacle_range_limit)){
        ROS_ERROR_THROTTLE(1.0, "Path collides with dynamic obstacles.");
        return false;
    }

    return true;
}



bool collisionCheckingAndRiskPredictionDynamic(const Eigen::MatrixXd &p, const Eigen::Vector3d &current_p, float &risk, double absolute_safe_time = 1.f, double start_time = 0.0, float dynamic_obstacle_range_limit = 5.f){
    // Filter out close obstacles
    vector<ActorState> close_actors;
    for(auto &actor : actor_avg_states){
        /// TODO: USE 3D distance
        double distance_2d = sqrt((current_p(0) - actor.px)*(current_p(0) - actor.px) +
                                  (current_p(1) - actor.py)*(current_p(1) - actor.py));
        if(distance_2d <= dynamic_obstacle_range_limit){
            close_actors.push_back(actor);
        }
    }

    // Calculate checking range
    int collision_checking_times = absolute_safe_time / CAL_DURATION;
    if(collision_checking_times > p.rows()){
        collision_checking_times = p.rows();
    }

    // Check collision
    vector<Eigen::Vector3d> p_vector;
    for(int i=0; i < collision_checking_times; ++i){
        p_vector.emplace_back(p.row(i));
    }
    if(!collisionCheckingWithoutRisk(p_vector, current_p, start_time, dynamic_obstacle_range_limit)){
        return false;
    }

    // Calculate future risk
    risk = 0.f;
    static const float risk_distance_threshold = 2 * pp.collision_threshold_dynamic;
    static const float risk_quad_function_coefficient = -1.f / (risk_distance_threshold * risk_distance_threshold);

    for(int i=collision_checking_times; i < p.rows(); ++i){
        float check_t = i * CAL_DURATION + start_time;
        float check_px = p(i, 0);
        float check_py = p(i, 1);
//        double check_pz = p(i, 2);

        float check_point_risk = 0.0;

        for(auto const &actor : close_actors){
            float actor_px_t = actor.px + actor.vx * check_t;
            float actor_py_t = actor.py + actor.vy * check_t;

            float check_distance_2d_square = (check_px-actor_px_t)*(check_px-actor_px_t) + (check_py-actor_py_t)*(check_py-actor_py_t);
            if(check_distance_2d_square < risk_distance_threshold * risk_distance_threshold){
                check_point_risk += risk_quad_function_coefficient * check_distance_2d_square + 1.f;

                if(check_point_risk > 1.0){
                    check_point_risk = 1.0;
                    break;
                }
            }
        }

        risk += check_point_risk;
    }

    // Consider total risk limitation
    if(risk > pp.dynamic_future_risk_threshold){
        return false;
    }

    // Set a risk limitation
    if(risk > MAX_RISK_ONE_PRIMITIVE){
        risk = MAX_RISK_ONE_PRIMITIVE;
    }

    return true;
}



bool trajectoryPlanOnePiece(const Eigen::Vector3d &p_start, const Eigen::Vector3d& v_start, const Eigen::Vector3d& a_start, float yaw_start,
                            Eigen::MatrixXd &traj_p_buffer_chosen, Eigen::MatrixXd &traj_v_buffer_chosen,
                            Eigen::MatrixXd &traj_a_buffer_chosen, int& connected_to_global, int &connected_point_global_seq,
                            float &angle_h_chosen, float &angle_v_chosen,
                            int piece_seq = 1, float absolute_safe_time = 1.f, double start_time = 0.0){

    static bool first_plan = true;
    static float last_angle_h = 0.f;
    static float last_angle_v = 0.f;

    vector<Eigen::MatrixXd> primitive_candidate_p;
    vector<Eigen::MatrixXd> primitive_candidate_v;
    vector<Eigen::MatrixXd> primitive_candidate_a;
    vector<float> primitive_candidate_risk;
    vector<int> primitive_candidate_cross_global_traj;
    vector<int> primitive_candidate_cross_point_seq;
    vector<float> primitive_candidate_global_traj_dist;
    vector<float> primitive_candidate_new_ref_length;
    vector<float> primitive_candidate_angle_h;
    vector<float> primitive_candidate_angle_v;

    Eigen::Vector3d delt_p = p_goal - p_start;
    float phi_h = atan2(delt_p(1), delt_p(0)); //% horizental offset angle
    float phi_v = atan2(delt_p(2), sqrt(delt_p(0) * delt_p(0) + delt_p(1) * delt_p(1))); //% vertical offset angle


    int i_max, j_max;
    if(piece_seq == 1){
        i_max = ANGLE_H_NUM_SPARSE;
        j_max = ANGLE_V_NUM_SPARSE;
    }else{
        i_max = ANGLE_H_NUM_DENSE;
        j_max = ANGLE_V_NUM_DENSE;
    }

    static int global_connection_start_checking_segment_seq = 1;
    /// To avoid connect to segments in the behind.
    if(global_trajectory_imported){
        if((global_traj_key_points[global_connection_start_checking_segment_seq-1] - p_goal).squaredNorm()
           > (p_start - p_goal).squaredNorm()){
            global_connection_start_checking_segment_seq += 1;
            ROS_WARN(" Global trajectory connection sequence moved. ");
        }
    }


    for(int i=0; i < i_max; i++)
    {
        for(int j=0; j < j_max; j++)
        {
            float angle_h_this, angle_v_this;
            if(piece_seq == 1) {
                angle_h_this = Angle_h_sparse(i) + phi_h;
                if(if_plan_vertical_path){
                    angle_v_this = Angle_v_sparse(j) + phi_v;
                }else{
                    angle_v_this = 0.f;
                }

            }else{ /// sample with dense angles if planning traj2.
                angle_h_this = Angle_h_dense(i) + phi_h;
                if(if_plan_vertical_path){
                    angle_v_this = Angle_v_dense(j) + phi_v;
                }else{
                    angle_v_this = 0.f;
                }
            }


            Eigen::MatrixXd p_matrix;
            Eigen::MatrixXd v_matrix;
            Eigen::MatrixXd a_matrix;
            Eigen::VectorXd t_vector;

            float delt_x = pp.d_ref*cos(angle_v_this)*cos(angle_h_this+yaw0);
            float delt_y = pp.d_ref*cos(angle_v_this)*sin(angle_h_this+yaw0);
            float delt_z = pp.d_ref*sin(angle_v_this);

            Eigen::Vector3d pf, vf, af;
            pf << p_start(0) + delt_x, p_start(1) + delt_y, p_start(2) + delt_z;

            /// Check if connected to goal position is possible
            double closest_goal_distance;
            bool connected_to_goal_position = false;

            Eigen::Vector3d closest_goal_point;
            pointToLineSegmentDistance(p_start, pf, p_goal, closest_goal_point, closest_goal_distance);
            if(closest_goal_distance < pp.connect_global_path_threshold) //use the same parameter as global path connection
            {
                Eigen::Vector3d zero_vector = Eigen::Vector3d::Zero();
                pf = p_goal;
                if(motion_primitives_with_table_va_fixed(p_start, v_start, a_start, CAL_DURATION, pf, zero_vector, zero_vector,
                                                         p_matrix, v_matrix, a_matrix, t_vector)){
                    float risk = 0.0;
                    if(collisionCheckingAndRiskPredictionDynamic(p_matrix, p_start,risk, absolute_safe_time, start_time)){
                        primitive_candidate_p.push_back(p_matrix);
                        primitive_candidate_v.push_back(v_matrix);
                        primitive_candidate_a.push_back(a_matrix);
                        primitive_candidate_risk.push_back(risk);
                        primitive_candidate_angle_h.push_back(angle_h_this);
                        primitive_candidate_angle_v.push_back(angle_v_this);
                        primitive_candidate_cross_global_traj.push_back(0);
                        primitive_candidate_cross_point_seq.push_back(0);
                        primitive_candidate_global_traj_dist.push_back(0);
                        primitive_candidate_new_ref_length.push_back(closest_goal_distance);
                    }
                    ROS_INFO_THROTTLE(1,"!!!! Found connected path!");
                    connected_to_goal_position = true;
                }
            }


            /// Search cross point with global trajectory
            bool cross_with_global_traj = false;
            float min_dist = 10000.f;

            static const float length_for_not_connected_path = pp.d_ref / 3.f;
            float cost_cal_ref_length = length_for_not_connected_path;

            int connect_point_global_seq_this = -1;

            if(piece_seq == 1){  // Don't consider connection for the first piece
                min_dist = 0.f; ///TODO: check if 0 or 10000 is fine
            }else if(global_trajectory_imported && !connected_to_goal_position){
                for(int n=global_connection_start_checking_segment_seq; n < global_traj_key_points_seq.size(); ++n)
                {
                    Eigen::Vector3d p3 = global_traj_key_points[n-1];
                    Eigen::Vector3d p4 = global_traj_key_points[n];

                    /// Pre-check
//                    static const double too_far_square_dist_threshold =  pp.d_ref * pp.d_ref * 4.0;
//                    if((p3 - p_start).squaredNorm() > too_far_square_dist_threshold && (p4 - p_start).squaredNorm() > too_far_square_dist_threshold){
//                        continue;
//                    }

                    /// Check by distance between line segments
                    Eigen::Vector3d l1_nearest_p,l2_nearest_p;
                    double distance = 100.0;
                    lineSegmentDistance(p_start, pf, p3, p4, l1_nearest_p, l2_nearest_p, distance);

                    if(distance < min_dist){
                        min_dist = (float)distance;
                    }

                    if(distance < pp.connect_global_path_threshold){
                        // find nearest point to l2_nearest_p from global trajectory
                        double d_min_temp_square = 10000.0;
                        for(int m=global_traj_key_points_seq[n-1]; m<global_traj_key_points_seq[n]; ++m){
                            Eigen::Vector3d p_this = global_traj_p[m];
                            double d_this_square = (p_this - l2_nearest_p).squaredNorm();
                            if(d_this_square < d_min_temp_square){
                                d_min_temp_square = d_this_square;
                                pf = p_this;
                                vf = global_traj_v[m];
                                af = global_traj_a[m];
                                connect_point_global_seq_this = m;
                            }
                        }
                        cross_with_global_traj = true;
                        cost_cal_ref_length = (pf - p_start).norm();

                        if(n+1 > global_connection_start_checking_segment_seq && n > 1){ /// To avoid checking from the first segment on the global path
                            global_connection_start_checking_segment_seq = n+1;
                        }

                        break;
                    }

                }
            }



            if(!cross_with_global_traj)
            {
                if(motion_primitives_with_table_va_free(p_start, v_start, a_start, pf, p_goal,
                                                        pp.v_max_at_goal_ori, CAL_DURATION, p_matrix, v_matrix, a_matrix, t_vector)){

                    // Collision checking and risk prediction
                    float risk = 0.0;
                    if(collisionCheckingAndRiskPredictionDynamic(p_matrix, p_start,risk, absolute_safe_time, start_time)){
                        primitive_candidate_p.push_back(p_matrix);
                        primitive_candidate_v.push_back(v_matrix);
                        primitive_candidate_a.push_back(a_matrix);
                        primitive_candidate_risk.push_back(risk);
                        primitive_candidate_angle_h.push_back(angle_h_this);
                        primitive_candidate_angle_v.push_back(angle_v_this);
                        primitive_candidate_cross_global_traj.push_back(0);
                        primitive_candidate_cross_point_seq.push_back(connect_point_global_seq_this);
                        primitive_candidate_global_traj_dist.push_back(min_dist);
                        primitive_candidate_new_ref_length.push_back(cost_cal_ref_length);
                    }
                }
            }else{

                // Will use the cross point as final state
                if(motion_primitives_with_table_va_fixed(p_start, v_start, a_start, CAL_DURATION, pf, vf, af,
                                                         p_matrix, v_matrix, a_matrix, t_vector))
                {
                    // Collision checking and risk prediction
                    float risk = 0.0;
                    if(collisionCheckingAndRiskPredictionDynamic(p_matrix,p_start, risk, absolute_safe_time, start_time)){
                        primitive_candidate_p.push_back(p_matrix);
                        primitive_candidate_v.push_back(v_matrix);
                        primitive_candidate_a.push_back(a_matrix);
                        primitive_candidate_risk.push_back(risk);
                        primitive_candidate_angle_h.push_back(angle_h_this);
                        primitive_candidate_angle_v.push_back(angle_v_this);
                        primitive_candidate_cross_global_traj.push_back(1);
                        primitive_candidate_cross_point_seq.push_back(connect_point_global_seq_this);
                        primitive_candidate_global_traj_dist.push_back(min_dist);
                        primitive_candidate_new_ref_length.push_back(cost_cal_ref_length);
                    }

                }

            }

        }
    }

    if(primitive_candidate_p.empty()){
        ROS_WARN("No safe path, piece_seq=%d", piece_seq);
        return false;
    }
//    else if(piece_seq == 2){
//        /// Display
//        int counter = 0;
//        for(auto &m : primitive_candidate_p){
//            marker_publish(m, 77 + counter, 0.6, 0.6, 0.8, 0.04, 10, 4);
//            ++counter;
//        }
//    }

    // Find the best trajectory
    float cost_min = 100000.f;

    for(int i=0; i<primitive_candidate_p.size(); ++i){
        float cost_goal = pp.k1_xy * (primitive_candidate_angle_h[i] - phi_h) * (primitive_candidate_angle_h[i] - phi_h) +
                          pp.k1_z * (primitive_candidate_angle_v[i] - phi_v) * (primitive_candidate_angle_v[i] - phi_v);

        float cost_direction_change = 0.f;
        if(!first_plan){
            if(piece_seq == 1){ // for the first piece
                float delt_angle_h = primitive_candidate_angle_h[i]  - last_angle_h;
                float delt_angle_v = primitive_candidate_angle_v[i]  - last_angle_v;
                cost_direction_change = pp.k2_xy * delt_angle_h*delt_angle_h + pp.k2_z * delt_angle_v*delt_angle_v;
            }else{ //for the first piece
                float delt_angle_h = primitive_candidate_angle_h[i] - (float)acos(v_start.y()/v_start.x());
                float delt_angle_v = 0; /// No vertical motion is treated to be better
                cost_direction_change = pp.k2_xy * delt_angle_h*delt_angle_h + pp.k2_z * delt_angle_v*delt_angle_v;
            }

        }

        float cost_near_global_traj = 0.f;
        float cost_long_ref_length = 0.f;
        if(piece_seq == 2 && global_trajectory_imported){
            cost_near_global_traj = pp.k_near_global_traj * primitive_candidate_global_traj_dist[i] * primitive_candidate_global_traj_dist[i];
            cost_long_ref_length = pp.k_long_dist_ref / (primitive_candidate_new_ref_length[i] * primitive_candidate_new_ref_length[i]);
//            ROS_INFO_THROTTLE(1, "cost_near_global_traj = %f, cost_long_ref_length = %f", cost_near_global_traj, cost_long_ref_length);
        }

        float total_cost = cost_goal + cost_direction_change + primitive_candidate_risk[i] + cost_near_global_traj + cost_long_ref_length;
        if(total_cost < cost_min){
            cost_min = total_cost;
            traj_p_buffer_chosen = primitive_candidate_p[i];
            traj_v_buffer_chosen = primitive_candidate_v[i];
            traj_a_buffer_chosen = primitive_candidate_a[i];
            connected_to_global = primitive_candidate_cross_global_traj[i];
            connected_point_global_seq = primitive_candidate_cross_point_seq[i];

            angle_h_chosen = primitive_candidate_angle_h[i];
            angle_v_chosen = primitive_candidate_angle_v[i];

        }
    }

    if(piece_seq == 1){  // only for the first piece
        first_plan = false;
        last_angle_h = angle_h_chosen;
        last_angle_v = angle_v_chosen;
    }

//    ROS_INFO("piece_seq=%d, primitive_candidate_p.size()=%ld, traj_p_buffer_chosen.size()=%ld", piece_seq, primitive_candidate_p.size(), traj_p_buffer_chosen.size());
    if(traj_p_buffer_chosen.rows() > 1){  ///TODO: check why sometimes traj_p_buffer_chosen.rows()  would be zero
        return true;
    }else{
        return false;
    }
}

int if_traj1_connected_to_global = 0;
int if_traj2_connected_to_global = 0;
int connected_point_global_seq_traj1 = 0;
int connected_point_global_seq_traj2 = 0;

void trajectoryCallback(const ros::TimerEvent& e) {

    /// Check if arrived at goal position
    if((p0-p_goal).norm() < 0.5){
        ROS_WARN_THROTTLE(1.0, "Arrived at goal position");
        planning_finished = true;  //to hover
    }
    if(planning_finished || !current_pose_received){
        return;
    }


    /// Start replanning
    double planning_start_time = ros::Time::now().toSec();

    static const int points_num_one_piece1 = (int)(ABSOLUTE_SAFE_TIME_PIECE1 / CAL_DURATION);
    static const int points_num_one_piece2 = (int)(ABSOLUTE_SAFE_TIME_PIECE2 / CAL_DURATION);

    Eigen::Vector3d current_p = p0;
    Eigen::Vector3d current_v = v0;
    Eigen::Vector3d current_a = a0;
    float current_yaw = yaw0;

    if(!trajectory_initialized){
        last_send_p = current_p;
        last_send_v << 0.0, 0.0, 0.0;
        last_send_a << 0.0, 0.0, 0.0;
    }


    /// For global path
    if(if_traj1_connected_to_global && !send_global_path){
        send_global_path = true;
        global_path_sending_seq = connected_point_global_seq_traj1;
        ROS_WARN("Using Global path");
        textPublish("Global path", 57, -7.f, -1.f, 2.f);
    }

    if(send_global_path){
        trajectory_initialized = true;
        while(traj_p_buffer_1.size() < traj1_points_num_to_keep_global + trajectory_1_send_counter){
            traj_p_buffer_1.push_back(global_traj_p[global_path_sending_seq]);
            traj_v_buffer_1.push_back(global_traj_v[global_path_sending_seq]);
            traj_a_buffer_1.push_back(global_traj_a[global_path_sending_seq]);
            ++ global_path_sending_seq;
        }
    }

    /// Left points piece one
    vector<Eigen::Vector3d> left_points_piece_one;
    for(int i=trajectory_1_send_counter; i<traj_p_buffer_1.size(); ++i){
        left_points_piece_one.emplace_back(traj_p_buffer_1[i]);
    }

    // Visualization
    marker_publish(left_points_piece_one, 3, 0.f, 1.f, 0.f, 0.3, 8, 8);


    /// Check if need to replace trajectory 1 with 2.
    static const int transfer_buffer_size = 5;
    // !send_global_path &&
    if(trajectory_initialized && trajectory_1_send_counter >= traj_p_buffer_1.size() - transfer_buffer_size){ /// frequency and calculation frequency usually don't match. Use buffer with transfer_buffer_size points.

        if(!traj_p_buffer_2.empty()){
            vector<Eigen::Vector3d> transfer_buffer_p_temp;
            vector<Eigen::Vector3d> transfer_buffer_v_temp;
            vector<Eigen::Vector3d> transfer_buffer_a_temp;
            for(int j= (int)traj_p_buffer_1.size() - trajectory_1_send_counter - 1; j>0; --j){
                transfer_buffer_p_temp.push_back(traj_p_buffer_1[traj_p_buffer_1.size() - j]);
                transfer_buffer_v_temp.push_back(traj_v_buffer_1[traj_v_buffer_1.size() - j]);
                transfer_buffer_a_temp.push_back(traj_a_buffer_1[traj_a_buffer_1.size() - j]);
            }

            traj_p_buffer_1 = transfer_buffer_p_temp;
            traj_v_buffer_1 = transfer_buffer_v_temp;
            traj_a_buffer_1 = transfer_buffer_a_temp;
            trajectory_1_send_counter = 0;

            int new_start_seq = traj_p_buffer_1.size();

            for(int i=0; i<traj_p_buffer_2.size(); ++i){  // 0 is ignore because it is the same point as the last point in original traj1
                traj_p_buffer_1.push_back(traj_p_buffer_2[i]);
                traj_v_buffer_1.push_back(traj_v_buffer_2[i]);
                traj_a_buffer_1.push_back(traj_a_buffer_2[i]);
            }

            if_traj1_connected_to_global = if_traj2_connected_to_global; // Pass the global path flag parameter.
            connected_point_global_seq_traj1 = connected_point_global_seq_traj2;

            trajectory_buffer_2_requires_update = true;

            ROS_INFO("Replaced Traj1 with Traj2, tracked_point=(%f, %f, %f), new_point=(%f, %f, %f), tracked_vel=(%f, %f, %f), new_vel=(%f, %f, %f)",
                     last_send_p(0),last_send_p(1),last_send_p(2), traj_p_buffer_1[0](0), traj_p_buffer_1[0](1), traj_p_buffer_1[0](2),
                     last_send_v(0), last_send_v(1), last_send_v(2), traj_v_buffer_1[0](0), traj_v_buffer_1[0](1), traj_v_buffer_1[0](2));

        }else{
            ROS_INFO("No valid Traj2 to replace, will replan traj1");
            trajectory_buffer_1_requires_update = true;
            trajectory_buffer_2_requires_update = true;
        }

    }

    /// Check if replanning is necessary
    static float replanning_tracking_error_threshold = 0.6f;
    static float start_point_tracking_error_threshold = 0.3f;

    if(!trajectory_buffer_1_requires_update){
        Eigen::Vector3d tracking_p_error = current_p - last_send_p;

        if(tracking_p_error.norm() > replanning_tracking_error_threshold){ //!send_global_path &&
            trajectory_buffer_1_requires_update = true;
            trajectory_buffer_2_requires_update = true;
            ROS_WARN_THROTTLE(0.1, "Replan traj1 because of tracking error. current_p=(%f,%f), sp=(%f,%f)", current_p.x(), current_p.y(), traj_p_buffer_1[trajectory_1_send_counter-1].x(), traj_p_buffer_1[trajectory_1_send_counter-1].y());

        }else{
            //check collision, if collision, replan
            if(!collisionCheckingWithoutRisk(left_points_piece_one, current_p)){
                trajectory_buffer_1_requires_update = true;
                trajectory_buffer_2_requires_update = true;

                send_global_path = false; // To replan locally
                ROS_WARN("Using local path");
                textPublish("Local path", 57, -7.f, -1.f, 2.f);

                ROS_INFO_THROTTLE(0.3, "Replan traj1 because of collision.");
            }

        }
    }

//    if(!send_global_path)
//    {
        // Check collision to determine if replan piece2 when piece2 is connected to global path.
        if(!trajectory_buffer_2_requires_update && if_traj2_connected_to_global){
            float piece2_risk = 0.f;
            bool piece2_free_flag = collisionCheckingAndRiskPredictionDynamic(traj_p_buffer_2_matrix, current_p, piece2_risk, ABSOLUTE_SAFE_TIME_PIECE2);  /// TODO: note, current_p is not accurate. Should be the first point of traj2

            if(!piece2_free_flag){
                trajectory_buffer_2_requires_update = true;
                ROS_INFO_THROTTLE(0.3, "Replan traj2 because of risk.");
            }
        }else{
            // Otherwise replan piece2 all the time
            trajectory_buffer_2_requires_update = true;
        }

//    trajectory_buffer_2_requires_update = true;
//    }


    static int traj2_plan_failed_times = 0;
    if(!send_global_path && traj2_plan_failed_times > 3){
        trajectory_buffer_1_requires_update = true;
        traj2_plan_failed_times = 0;
    }

    /// Update piece 1
    static float angle_h_piece_1 = 0.f;
    static float angle_v_piece_1 = 0.f;

    if(trajectory_buffer_1_requires_update){
        Eigen::MatrixXd traj_p_buffer_chosen, traj_v_buffer_chosen, traj_a_buffer_chosen;

        Eigen::Vector3d start_p, start_v, start_a;
        if(trajectory_initialized){
            Eigen::Vector3d tracking_p_error = current_p - last_send_p;
            if(tracking_p_error.norm() > start_point_tracking_error_threshold){
                start_p = current_p * 0.7 + last_send_p * 0.3;
                start_v = current_v * 0.7 + last_send_v * 0.3;
                start_a = current_a * 0.7 + last_send_a * 0.3;
            }else{
                start_p = last_send_p;
                start_v = last_send_v;
                start_a = last_send_a;
            }
        }else{
            start_p = current_p;
            start_v = current_v;
            start_a = current_a;
        }

        ROS_INFO_THROTTLE(0.5, "Start =(%f, %f, %f), Start v =(%f, %f, %f)", start_p(0), start_p(1), start_p(2), start_v(0), start_v(1), start_v(2));

        traj_p_buffer_1.clear();
        traj_v_buffer_1.clear();
        traj_a_buffer_1.clear();

        float angle_h_chosen = 0.f, angle_v_chosen = 0.f;
        bool plan_successful_flag = trajectoryPlanOnePiece(start_p, start_v, start_a, 0.0, traj_p_buffer_chosen, traj_v_buffer_chosen, traj_a_buffer_chosen,
                                                           if_traj1_connected_to_global, connected_point_global_seq_traj1, angle_h_chosen, angle_v_chosen, 1, ABSOLUTE_SAFE_TIME_PIECE1);

        angle_h_piece_1 = angle_h_chosen;
        angle_v_piece_1 = angle_v_chosen;

        if(!if_traj1_connected_to_global){
            send_global_path = false;
        }else{  /// In current version, if_traj1_connected_to_global can only be false;
            send_global_path = true;
        }

        if(plan_successful_flag){
            int point_num = points_num_one_piece1;
            if(point_num > traj_p_buffer_chosen.rows()){point_num = traj_p_buffer_chosen.rows();}

            for(int i=0; i<point_num; ++i){
                traj_p_buffer_1.emplace_back(traj_p_buffer_chosen.row(i));
                traj_v_buffer_1.emplace_back(traj_v_buffer_chosen.row(i));
                traj_a_buffer_1.emplace_back(traj_a_buffer_chosen.row(i));
                trajectory_1_send_counter = 0;
            }

//            whole_traj1_to_visualize = traj_p_buffer_chosen;

            trajectory_buffer_1_requires_update = false;
            safe_trajectory_avaliable = true;
            trajectory_initialized = true;
        }
        else{
            /// TODO: Should enter safety mode
            safe_trajectory_avaliable = false;
            return;
        }

    }

    /// Update piece 2
    if(trajectory_buffer_2_requires_update && safe_trajectory_avaliable){

        Eigen::MatrixXd traj_p_buffer_chosen, traj_v_buffer_chosen, traj_a_buffer_chosen;

        traj_p_buffer_2.clear();
        traj_v_buffer_2.clear();
        traj_a_buffer_2.clear();

        float angle_h_chosen = 0.f, angle_v_chosen = 0.f;
        bool plan_successful_flag = trajectoryPlanOnePiece(traj_p_buffer_1[traj_p_buffer_1.size()-1], traj_v_buffer_1[traj_v_buffer_1.size()-1],
                                                           traj_a_buffer_1[traj_a_buffer_1.size()-1], 0.0, traj_p_buffer_chosen, traj_v_buffer_chosen, traj_a_buffer_chosen,
                                                           if_traj2_connected_to_global, connected_point_global_seq_traj2, angle_h_chosen, angle_v_chosen, 2, ABSOLUTE_SAFE_TIME_PIECE2, left_points_piece_one.size()*CAL_DURATION);

        if(plan_successful_flag){
            if(if_traj2_connected_to_global){
                textPublish("Connected to global path", 11, 0, 0, 6);
            }else{
                textPublish("Not connected", 11, 0, 0, 6);
            }

            int point_num = points_num_one_piece2;
            if(if_traj2_connected_to_global || point_num > traj_p_buffer_chosen.rows()){point_num = traj_p_buffer_chosen.rows();}

            for(int i=0; i<point_num; ++i){
                traj_p_buffer_2.emplace_back(traj_p_buffer_chosen.row(i));
                traj_v_buffer_2.emplace_back(traj_v_buffer_chosen.row(i));
                traj_a_buffer_2.emplace_back(traj_a_buffer_chosen.row(i));
            }

            /*** Check if direct connection from current point to the end of piece 2 is safe ***/
            if( fabs(angle_h_chosen - angle_h_piece_1) >  1.05f ||  fabs(angle_v_chosen - angle_v_piece_1) > 0.52f) // h: sixty degrees, v: thirty degrees
            {
                Eigen::Vector3d start_p, start_v, start_a;
                if(trajectory_initialized){
                    Eigen::Vector3d tracking_p_error = current_p - last_send_p;
                    if(tracking_p_error.norm() > start_point_tracking_error_threshold){
                        start_p = current_p * 0.7 + last_send_p * 0.3;
                        start_v = current_v * 0.7 + last_send_v * 0.3;
                        start_a = current_a * 0.7 + last_send_a * 0.3;
                    }else{
                        start_p = last_send_p;
                        start_v = last_send_v;
                        start_a = last_send_a;
                    }
                }else{
                    start_p = current_p;
                    start_v = current_v;
                    start_a = current_a;
                }

                Eigen::MatrixXd p_matrix;
                Eigen::MatrixXd v_matrix;
                Eigen::MatrixXd a_matrix;
                Eigen::VectorXd t_vector;

                Eigen::Vector3d pf, vf, af;

                pf << traj_p_buffer_chosen(point_num-1, 0), traj_p_buffer_chosen(point_num-1, 1), traj_p_buffer_chosen(point_num-1, 2);
                vf << traj_v_buffer_chosen(point_num-1, 0), traj_v_buffer_chosen(point_num-1, 1), traj_v_buffer_chosen(point_num-1, 2);
                af << traj_a_buffer_chosen(point_num-1, 0), traj_a_buffer_chosen(point_num-1, 1), traj_a_buffer_chosen(point_num-1, 2);

                if(motion_primitives_with_table_va_fixed(start_p, start_v, start_a, CAL_DURATION,
                                                         pf, vf, af, p_matrix, v_matrix, a_matrix, t_vector))
                {
                    float risk = 0.0;
                    if(collisionCheckingAndRiskPredictionDynamic(p_matrix, start_p, risk, ABSOLUTE_SAFE_TIME_PIECE1+ABSOLUTE_SAFE_TIME_PIECE2)){
                        if(risk < MAX_RISK_ONE_PRIMITIVE){
                            ROS_INFO("Replan successfully to avoid large turning. ***** ");

                            traj_p_buffer_1.clear();
                            traj_v_buffer_1.clear();
                            traj_a_buffer_1.clear();
                            traj_p_buffer_2.clear();
                            traj_v_buffer_2.clear();
                            traj_a_buffer_2.clear();

                            // Update piece 1
                            int piece_1_num = p_matrix.rows() / 2;
                            for(int ii=0; ii<piece_1_num; ++ii){
                                traj_p_buffer_1.emplace_back(p_matrix.row(ii));
                                traj_v_buffer_1.emplace_back(v_matrix.row(ii));
                                traj_a_buffer_1.emplace_back(a_matrix.row(ii));
                                trajectory_1_send_counter = 0;
                            }

                            // Update piece 2
                            for(int ii=piece_1_num; ii<p_matrix.rows(); ++ii){
                                traj_p_buffer_2.emplace_back(p_matrix.row(ii));
                                traj_v_buffer_2.emplace_back(v_matrix.row(ii));
                                traj_a_buffer_2.emplace_back(a_matrix.row(ii));
                            }

                        }
                    }
                }

            }
            /*** end  ***/


            trajectory_buffer_2_requires_update = false;
//            traj_p_buffer_2_matrix = traj_p_buffer_chosen;
            traj2_plan_failed_times = 0;


//            whole_traj2_to_visualize = traj_p_buffer_chosen;
        }else{
            ++ traj2_plan_failed_times;
        }

    }


    marker_publish(traj_p_buffer_1, 1, 0.8f, 0.8f, 0.8f, 0.1, 8);
    marker_publish(traj_p_buffer_2, 2, 0.2f, 0.6f, 0.7f, 0.1, 10);

//    if(whole_traj1_to_visualize.rows() > 0) marker_publish(whole_traj1_to_visualize, 4, 0.6f, 0.5f, 0.1f, 0.06, 10);
//    if(whole_traj2_to_visualize.rows() > 0) marker_publish(whole_traj2_to_visualize, 5, 0.8f, 0.8f, 0.8f, 0.06, 10);

    /// Yaw setpoint
    if(safe_trajectory_avaliable){
        if(left_points_piece_one.size() >= 5){
            yaw_sp = atan2(left_points_piece_one[5].y()-current_p.y(), left_points_piece_one[5].x()-current_p.x());
            static double max_delt_yaw = 0.05;
            if(yaw_sp > yaw_sp_last + max_delt_yaw){
                yaw_sp = yaw_sp_last + max_delt_yaw;
            }else if(yaw_sp < yaw_sp_last - max_delt_yaw){
                yaw_sp = yaw_sp_last - max_delt_yaw;
            }
        }else{
            yaw_sp = yaw_sp_last;
        }
        yaw_sp_last = yaw_sp;
    }

    trajectory_planned = true;
    double planning_end_time = ros::Time::now().toSec();
    ROS_INFO_THROTTLE(0.5, "Planning time is %lf s", planning_end_time-planning_start_time);


    /// Publish global trajectory
    if(global_trajectory_imported){
        const static float map_half_height = MAP_HEIGHT_VOXEL_NUM * VOXEL_RESOLUTION / 2.f;
        vector<Eigen::Vector3d> global_traj_p_moved, global_traj_key_points_moved;
        for(const auto &p : global_traj_p){
            Eigen::Vector3d p_moved = p - map_center_point;
            p_moved.z() = p.z();
            global_traj_p_moved.push_back(p_moved);
        }
        for(const auto &p : global_traj_key_points){
            Eigen::Vector3d p_moved = p - map_center_point;
            p_moved.z() = p.z();
            global_traj_key_points_moved.push_back(p_moved);
        }

        marker_publish(global_traj_p_moved, 88, 1.0, 0.0, 0.0, 0.05, global_traj_p.size(), 4);
        marker_publish(global_traj_key_points_moved, 89, 1.0, 0.0, 0.0, 0.1, global_traj_key_points.size(), 8);
    }

}


void sendCallback(const ros::TimerEvent& e)
{
    if(!trajectory_initialized){
        return;
    }

    static Eigen::Vector3d p_store_for_em;
    static float yaw_store_for_em = 0.f;

    if(planning_finished){  //Planning finished
        p_store_for_em  = p_goal;
        yaw_store_for_em = yaw0;

        safe_trajectory_avaliable = false;
        ROS_ERROR_THROTTLE(1, "Planning finished!");
    }

    static bool init_flag = true;
    if(init_flag){
        p_store_for_em << p0(0), p0(1), p0(2);
        yaw_store_for_em = yaw0;

        init_flag = false;
    }

    static float def_ori = pp.d_ref;

    trajectory_msgs::JointTrajectoryPoint pva_setpoint;

    float z_p_set, z_v_set, z_a_set;
    if(if_plan_vertical_path){
        z_p_set = traj_p_buffer_1[trajectory_1_send_counter](2);
        z_v_set = traj_v_buffer_1[trajectory_1_send_counter](2);
        z_a_set = traj_a_buffer_1[trajectory_1_send_counter](2);
    }
    else{
        z_p_set = p_goal(2);
        z_v_set = 0.f;
        z_a_set = 0.f;
    }

    if(safe_trajectory_avaliable && trajectory_1_send_counter < traj_p_buffer_1.size()){
        pp.d_ref = def_ori;

        pva_setpoint.positions.push_back(traj_p_buffer_1[trajectory_1_send_counter](0)); //x
        pva_setpoint.positions.push_back(traj_p_buffer_1[trajectory_1_send_counter](1)); //y
        pva_setpoint.positions.push_back(z_p_set); //z
        pva_setpoint.positions.push_back(yaw_sp);  //yaw
//        /// Banned yaw setpoint
//        pva_setpoint.positions.push_back(0);

        pva_setpoint.velocities.push_back(traj_v_buffer_1[trajectory_1_send_counter](0));
        pva_setpoint.velocities.push_back(traj_v_buffer_1[trajectory_1_send_counter](1));
        pva_setpoint.velocities.push_back(z_v_set);

        pva_setpoint.accelerations.push_back(traj_a_buffer_1[trajectory_1_send_counter](0));
        pva_setpoint.accelerations.push_back(traj_a_buffer_1[trajectory_1_send_counter](1));
        pva_setpoint.accelerations.push_back(z_a_set);

        p_store_for_em << p0(0), p0(1), p0(2);
        yaw_store_for_em = yaw0;

        ++ trajectory_1_send_counter;
        if(trajectory_1_send_counter >= traj_a_buffer_1.size()){
            -- trajectory_1_send_counter;
            safe_trajectory_avaliable = false;
            ROS_WARN_THROTTLE(0.5, "No points in the buffer.");
        }

        last_send_p = traj_p_buffer_1[trajectory_1_send_counter];
        last_send_v = traj_v_buffer_1[trajectory_1_send_counter];
        last_send_a = traj_a_buffer_1[trajectory_1_send_counter];

        textPublish("Normal flight", 55, -7.f, -1.f, 0.f);

    }else{  //emergency stop
        pp.d_ref = 0.5;

        pva_setpoint.positions.push_back(p_store_for_em(0)); //x
        pva_setpoint.positions.push_back(p_store_for_em(1)); //y
        pva_setpoint.positions.push_back(z_p_set); //z
        pva_setpoint.positions.push_back(yaw_store_for_em);  //yaw

        pva_setpoint.velocities.push_back(0);
        pva_setpoint.velocities.push_back(0);
        pva_setpoint.velocities.push_back(0);

        pva_setpoint.accelerations.push_back(0);
        pva_setpoint.accelerations.push_back(0);
        pva_setpoint.accelerations.push_back(0);

        ROS_WARN_THROTTLE(0.5, "Safety mode.");

        last_send_p = p_store_for_em;
        last_send_v << 0.0, 0.0, 0.0;
        last_send_a << 0.0, 0.0, 0.0;

        textPublish("Safety Mode", 55, -7.f, -1.f, 0.f);
    }

    pva_pub.publish(pva_setpoint);
}


int inRange(float &low, float &high, float &x)
{
    if(x > low && x < high){
        return 1;
    }else{
        return 0;
    }
}

void colorAssign(int &r, int &g, int &b, float v, float value_min=0.f, float value_max=1.f, int reverse_color=0)
{
    v = std::max(v, value_min);
    v = std::min(v, value_max);

    float v_range = value_max - value_min;
    int value = floor((v - value_min) / v_range * 240); // Mapping 0~1.0 to 0~240
    value = std::min(value, 240);

    if(reverse_color){
        value = 240 - value;
    }

    int section = value / 60;
    float float_key = (value % 60) / (float)60 * 255;
    int key = floor(float_key);
    int nkey = 255 - key;

    switch(section) {
        case 0: // G increase
            r = 255;
            g = key;
            b = 0;
            break;
        case 1: // R decrease
            r = nkey;
            g = 255;
            b = 0;
            break;
        case 2: // B increase
            r = 0;
            g = 255;
            b = key;
            break;
        case 3: // G decrease
            r = 0;
            g = nkey;
            b = 255;
            break;
        case 4:
            r = 0;
            g = 0;
            b = 255;
            break;
        default: // White
            r = 255;
            g = 255;
            b = 255;
    }

}



static void split(const string& s, vector<string>& tokens, const string& delimiters = " ")
{
    string::size_type lastPos = s.find_first_not_of(delimiters, 0);
    string::size_type pos = s.find_first_of(delimiters, lastPos);
    while (string::npos != pos || string::npos != lastPos) {
        tokens.push_back(s.substr(lastPos, pos - lastPos));
        lastPos = s.find_first_not_of(delimiters, pos);
        pos = s.find_first_of(delimiters, lastPos);
    }
}


void boxPoseCallback(const geometry_msgs::PoseStamped& msg)
{
    static_obstacles_vector[0].record_time = msg.header.stamp.toSec();
    static_obstacles_vector[0].px = msg.pose.position.x;
    static_obstacles_vector[0].py = msg.pose.position.y;
    static_obstacles_vector[0].pz = msg.pose.position.z;
    static_obstacles_vector[0].name = "box";
}


void purplePoseCallback(const geometry_msgs::PoseStamped& msg)
{
    static_obstacles_vector[1].record_time = msg.header.stamp.toSec();
    static_obstacles_vector[1].px = msg.pose.position.x;
    static_obstacles_vector[1].py = msg.pose.position.y;
    static_obstacles_vector[1].pz = msg.pose.position.z;
    static_obstacles_vector[1].name = "purple";
}


void fourWheelPoseCallback(const geometry_msgs::PoseStamped& msg)
{
    actor_avg_states[1].record_time = msg.header.stamp.toSec();
    actor_avg_states[1].px = msg.pose.position.x;
    actor_avg_states[1].py = msg.pose.position.y;
    actor_avg_states[1].pz = msg.pose.position.z;
    actor_avg_states[1].name = "fourWheel";

    Eigen::Quaternionf q;
    q.x() = msg.pose.orientation.x;
    q.y() = msg.pose.orientation.y;
    q.z() = msg.pose.orientation.z;
    q.w() = msg.pose.orientation.w;

    actor_avg_states[1].yaw = atan2(2*(q.w()*q.z()+q.x()*q.y()), 1-2*(q.z()*q.z()+q.y()*q.y()));
}



void fourWheelVelCallback(const geometry_msgs::TwistStamped& msg)
{
    actor_avg_states[1].record_time = msg.header.stamp.toSec();
    actor_avg_states[1].vx = msg.twist.linear.x;
    actor_avg_states[1].vy = msg.twist.linear.y;
    actor_avg_states[1].vz = msg.twist.linear.z;
}


void threeWheelPoseCallback(const geometry_msgs::PoseStamped& msg)
{
    actor_avg_states[0].record_time = msg.header.stamp.toSec();
    actor_avg_states[0].px = msg.pose.position.x;
    actor_avg_states[0].py = msg.pose.position.y;
    actor_avg_states[0].pz = msg.pose.position.z;
    actor_avg_states[0].name = "three_wheel";

    Eigen::Quaternionf q;
    q.x() = msg.pose.orientation.x;
    q.y() = msg.pose.orientation.y;
    q.z() = msg.pose.orientation.z;
    q.w() = msg.pose.orientation.w;

    actor_avg_states[0].yaw = atan2(2*(q.w()*q.z()+q.x()*q.y()), 1-2*(q.z()*q.z()+q.y()*q.y()));
}

void threeWheelVelCallback(const geometry_msgs::TwistStamped& msg)
{
    actor_avg_states[0].record_time = msg.header.stamp.toSec();
    actor_avg_states[0].vx = msg.twist.linear.x;
    actor_avg_states[0].vy = msg.twist.linear.y;
    actor_avg_states[0].vz = msg.twist.linear.z;
}

void positionCallback(const geometry_msgs::PoseStamped& msg)
{
    if(!state_locked)
    {
        state_locked = true;

        if(!if_in_simulation){ //real
            p0(0) = msg.pose.position.x;
            p0(1) = msg.pose.position.y;
            p0(2) = msg.pose.position.z;

            quad.x() = msg.pose.orientation.x;
            quad.y() = msg.pose.orientation.y;
            quad.z() = msg.pose.orientation.z;
            quad.w() = msg.pose.orientation.w;

            Eigen::Quaternionf axis; //= quad * q1 * quad.inverse();
            axis.w() = cos(PI_2/2.0);
            axis.x() = 0.0;
            axis.y() = 0.0;
            axis.z() = sin(PI_2/2.0);
            quad = quad * axis;
        }else{
            // NWU
            p0(0) = msg.pose.position.x;
            p0(1) = msg.pose.position.y;
            p0(2) = msg.pose.position.z;

            quad.x() = msg.pose.orientation.x;
            quad.y() = msg.pose.orientation.y;
            quad.z() = msg.pose.orientation.z;
            quad.w() = msg.pose.orientation.w;
        }


        // Queue for synchronization
        pose_timestamp_queue.push(msg.header.stamp.toSec());
        pos_queue.push(p0);
        att_queue.push(quad);

        if(pose_timestamp_queue.size()>200){
            pose_timestamp_queue.pop();
            pos_queue.pop();
            att_queue.pop();
        }


        /// Update yaw0 here, should be among [-PI, PI]
        // yaw0 = atan2(2*(quad.w()*quad.z()+quad.x()*quad.y()), 1-2*(quad.z()*quad.z()+quad.y()*quad.y()));   /// TODO: CHECK if this is right!!!
        yaw0 = atan2(2*(quad.w()*quad.z()+quad.x()*quad.y()), 1-2*(quad.z()*quad.z()+quad.y()*quad.y()));
        ROS_INFO_THROTTLE(1.0, "yaw = %f", yaw0);

        current_pose_received = true;
        state_locked = false;
    }
    /// Correct positioning offset
//    if(!offboard_ready){
//        init_p = p0;  //record a base point
//        p_goal(0) = p_goal_raw(0) + init_p(0);
//        p_goal(1) = p_goal_raw(1) + init_p(1);
//        p_goal(2) = init_p(2);
//        ROS_INFO_THROTTLE(1.0, "Waiting for offboard mode. p_goal = %f, %f, %f", p_goal(0), p_goal(1), p_goal(2));
//    }else{
//        ROS_INFO_THROTTLE(1.0, "IN offboard mode. pgoal = %f, %f, %f", p_goal(0), p_goal(1), p_goal(2));
//    }

//    ROS_INFO_THROTTLE(1.0, "Current pose. p0 = %f, %f, %f", p0(0), p0(1), p0(2));

    /// Global path visualization
//    vector<Eigen::Vector3d> global_traj_key_points_moved;
//    for(auto &p : global_traj_key_points){
//        Eigen::Vector3d p_moved = p - p0;
//        global_t raj_key_points_moved.push_back(p_moved);
//    }
//    int id, float r, float g, float b, float width, int publish_num, int type
//    marker_publish(global_traj_key_points, 67, 0.8, 0.2, 0.2, 0.2, global_traj_key_points.size(), 4);
}

void velocityCallback(const geometry_msgs::TwistStamped& msg)
{
    if(!state_locked)
    {
        state_locked = true;

        if(!if_in_simulation){
            // /** Change from ENU to NWU, NEEDS CAREFUL CHECKING!!!!, chg**/
            v0(0) = msg.twist.linear.x;
            v0(1) = msg.twist.linear.y;
            v0(2) = msg.twist.linear.z;
            yaw0_rate = msg.twist.angular.z;
        }else{
            /**  NWU  **/
            v0(0) = msg.twist.linear.x;
            v0(1) = msg.twist.linear.y;
            v0(2) = msg.twist.linear.z;
            yaw0_rate = msg.twist.angular.z;
        }


        if(fabs(v0(0)) < 0.05) v0(0) = 0.0;  //add a dead zone for v0 used in motion primatives
        if(fabs(v0(1)) < 0.05) v0(1) = 0.0;  //add a dead zone for v0 used in motion primatives
        if(fabs(v0(2)) < 0.05) v0(2) = 0.0;  //add a dead zone for v0 used in motion primatives

        float max_current_v = 0.8;
        for(int i=0; i<3; i++){
            if(v0(i) < -max_current_v){
                v0(i) = -max_current_v;
            }else if(v0(i) > max_current_v){
                v0(i) = max_current_v;
            }
        }

        /** Calculate virtual accelerates from velocity. Original accelerates given by px4 is too noisy **/
        static bool init_v_flag = true;
        static float last_time, last_vx, last_vy, last_vz;

        if(init_v_flag){
            init_v_flag = false;
        }
        else{
            double delt_t = ros::Time::now().toSec() - last_time;
            a0(0) = (v0(0) - last_vx) / delt_t;
            a0(1) = (v0(1) - last_vy) / delt_t;
            a0(2) = (v0(2) - last_vz) / delt_t;

            if(fabs(a0(0)) < 0.2) a0(0) = 0.0;  //dead zone for acc x
            if(fabs(a0(1)) < 0.2) a0(1) = 0.0; //dead zone for acc y
            if(fabs(a0(2)) < 0.2) a0(2) = 0.0; //dead zone for acc z

            float max_current_a = 1.0;
            for(int i=0; i<3; i++){
                if(a0(i) < -max_current_a){
                    a0(i) = -max_current_a;
                }else if(a0(i) > max_current_a){
                    a0(i) = max_current_a;
                }
            }

            //ROS_INFO("acc=(%f, %f, %f)", a0(0), a0(1), a0(2));
        }

        last_time = ros::Time::now().toSec();
        last_vx = v0(0);
        last_vy = v0(1);
        last_vz = v0(2);

        state_locked = false;
    }
}




void uavModeCallback(const mavros_msgs::State &msg)
{
    if(msg.mode=="OFFBOARD") offboard_ready=true;
    else offboard_ready=false;
}


void marker_publish(const Eigen::MatrixXd &Points, int id, float r, float g, float b, float width, int publish_num, int type)
{
//    uint8 ARROW=0//箭头
//    uint8 CUBE=1//立方体
//    uint8 SPHERE=2//球
//    uint8 CYLINDER=3//圆柱体
//    uint8 LINE_STRIP=4//线条（点的连线）
//    uint8 LINE_LIST=5//线条序列
//    uint8 CUBE_LIST=6//立方体序列
//    uint8 SPHERE_LIST=7//球序列
//    uint8 POINTS=8//点集
//    uint8 TEXT_VIEW_FACING=9//显示3D的文字
//    uint8 MESH_RESOURCE=10//网格？
//    uint8 TRIANGLE_LIST=11//三角形序列

    if(Points.rows() < 1) return;

    visualization_msgs::Marker points;
    points.header.frame_id = "map";
    points.header.stamp = ros::Time::now();
    points.action = visualization_msgs::Marker::ADD;
    points.ns = "lines_and_points";
    points.id = id;
    points.type = type;

    // Line width
    points.scale.x = width;
    points.scale.y = width;

    points.color.r = r;
    points.color.g = g;
    points.color.b = b;
    points.color.a = 1.0;
    points.lifetime = ros::Duration(0);

    int step = 1;
    if(publish_num > 0){
        step = (int)Points.rows() / publish_num;
    }

    for(int i=0; i<Points.rows(); i+=step)
    {
        geometry_msgs::Point p;
        p.x = Points(i, 0) - map_center_point.x();
        p.y = Points(i, 1) - map_center_point.y();
        p.z = Points(i, 2) - map_center_point.z();

        points.points.push_back(p);
    }

    geometry_msgs::Point p;
    p.x = Points(Points.rows()-1, 0) - map_center_point.x();
    p.y = Points(Points.rows()-1, 1) - map_center_point.y();
    p.z = Points(Points.rows()-1, 2) - map_center_point.z();
    points.points.push_back(p);

    current_marker_pub.publish(points);
}


void marker_publish(const vector<Eigen::Vector3d> &Points, int id, float r, float g, float b, float width, int publish_num, int type)
{
//    uint8 ARROW=0//箭头
//    uint8 CUBE=1//立方体
//    uint8 SPHERE=2//球
//    uint8 CYLINDER=3//圆柱体
//    uint8 LINE_STRIP=4//线条（点的连线）
//    uint8 LINE_LIST=5//线条序列
//    uint8 CUBE_LIST=6//立方体序列
//    uint8 SPHERE_LIST=7//球序列
//    uint8 POINTS=8//点集
//    uint8 TEXT_VIEW_FACING=9//显示3D的文字
//    uint8 MESH_RESOURCE=10//网格？
//    uint8 TRIANGLE_LIST=11//三角形序列

//    if(Points.empty()) return;

    visualization_msgs::Marker points;
    points.header.frame_id = "map";
    points.header.stamp = ros::Time::now();
    points.action = visualization_msgs::Marker::ADD;
    points.ns = "lines_and_points";
    points.id = id;
    points.type = type;

    // Line width
    points.scale.x = width;
    points.scale.y = width;

    points.color.r = r;
    points.color.g = g;
    points.color.b = b;
    points.color.a = 1.0;
    points.lifetime = ros::Duration(0);


    if(!Points.empty()){
        int step = 1;
        if(publish_num > 0){
            step = Points.size() / publish_num;
        }
        step = max(1, step);


        for(int i=0; i<Points.size(); i+=step)
        {
            geometry_msgs::Point p;
            p.x = Points[i](0) - map_center_point.x();
            p.y = Points[i](1) - map_center_point.y();
            p.z = Points[i](2) - map_center_point.z();
            points.points.push_back(p);
        }

        geometry_msgs::Point p;
        p.x = Points[Points.size()-1](0) - map_center_point.x();
        p.y = Points[Points.size()-1](1) - map_center_point.y();
        p.z = Points[Points.size()-1](2) - map_center_point.z();
        points.points.push_back(p);
    }else{
        // Let the marker published last time vanish
        for(int i=0; i<publish_num; ++i){
            geometry_msgs::Point p;
            p.x = 10000.f;
            p.y = 10000.f;
            p.z = 10000.f;
            points.points.push_back(p);
        }
    }


    current_marker_pub.publish(points);
}


void textPublish(string str, int id, float px, float py, float pz){
    visualization_msgs::Marker text;
    text.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
    text.ns = "lines_and_points";
    text.header.frame_id = "map";
    text.header.stamp = ros::Time::now();
    text.pose.orientation.w = 1.0;
    text.id = id;
    text.scale.x = 1.5;
    text.scale.y = 1.5;
    text.scale.z = 1.5;
    text.color.b = 1.0;
    text.color.g = 0.0;
    text.color.r = 0.0;
    text.color.a = 1.0;

    geometry_msgs::Pose pose;
    pose.position.x = px;
    pose.position.y = py;
    pose.position.z = pz;

    text.text = str;
    text.pose = pose;
    current_marker_pub.publish(text);
}


bool readCSVTraj(string filename, vector<vector<double>> &sp)
{
    ifstream infile(filename);
    if(infile.fail()){
        cout << "Can not find this file" << endl;
        return false;
    }

    string line;
    while(getline(infile, line)){
        istringstream in_line(line);
        vector<string> wordsInLine;
        string word;
        while(getline(in_line, word, ',')){
            wordsInLine.push_back(word);
        }

        vector<double> sp_one_line;
        for(auto & i : wordsInLine){
            sp_one_line.push_back(strtod(i.c_str(), nullptr));
        }
        sp.push_back(sp_one_line);
    }

    if(!sp.empty()){
        return true;
    }else{
        cout << "Found nothing in the file" << endl;
        return false;
    }
}


void setGlobalTrajectoryKeyPoints()
{
    const int key_point_interval_max = 10;
    const double key_point_angle_cos_threshold = 0.707; //45 degrees

    /// Add origin and the first key point
    int key_points_counter = 0;
    global_traj_key_points.push_back(global_traj_p[0]);
    global_traj_key_points_seq.push_back(0);

    int first_key_point_seq = key_point_interval_max/4;

    // In case hover set points are given
    while(first_key_point_seq < global_traj_p.size() && (global_traj_p[first_key_point_seq] - global_traj_p[0]).norm() < 0.001){
        first_key_point_seq += key_point_interval_max/4;
    }

    global_traj_key_points.push_back(global_traj_p[first_key_point_seq]);
    global_traj_key_points_seq.push_back(first_key_point_seq);
    key_points_counter += 1;

    Eigen::Vector3d last_key_vector = global_traj_key_points[key_points_counter] - global_traj_key_points[key_points_counter-1];

    /// Add the rest key points
    int selection_seq = 0;
    int last_key_point_seq = first_key_point_seq;

    for(int i=first_key_point_seq+1; i<global_traj_p.size(); ++i){
        Eigen::Vector3d p_this = global_traj_p[i];
        Eigen::Vector3d vector_this = p_this - global_traj_key_points[key_points_counter];
        if(vector_this.norm() < 0.001){
            continue;
        }

        ++ selection_seq;
        if(selection_seq >= key_point_interval_max || vector_this.dot(last_key_vector) / (vector_this.norm()*last_key_vector.norm()) < key_point_angle_cos_threshold){
            selection_seq = 0;
            global_traj_key_points.push_back(p_this);
            global_traj_key_points_seq.push_back(i);

            key_points_counter += 1;
            last_key_point_seq = i;

            last_key_vector = global_traj_key_points[key_points_counter] - global_traj_key_points[key_points_counter-1];
        }
    }

    /// Add last key point
    if(last_key_point_seq < global_traj_p.size()-1){
        global_traj_key_points.push_back(global_traj_p[global_traj_p.size()-1]);
        global_traj_key_points_seq.push_back(global_traj_p.size()-1);

        key_points_counter += 1;
        last_key_point_seq = (int)global_traj_p.size()-1;
    }

    /// Save key points analyze
    ofstream outFile;
    outFile.open("global_path_key_points.csv", ios::out);
    for(auto & global_traj_key_point : global_traj_key_points){
        outFile << global_traj_key_point.x() << "," << global_traj_key_point.y() << "," << global_traj_key_point.z() << "\n";
    }
    outFile.close();
    ROS_INFO("Trajectory key points saved!");

    /// Display
    ROS_INFO("Keypoints seq are: ");
    for(auto &p : global_traj_key_points_seq){
        cout << p << ", ";
    }
    cout << endl;
    
}


void getParameterList(const ros::NodeHandle& nh) {
    nh.getParam("/planning_return_global/p_goal_x", p_goal(0));
    nh.getParam("/planning_return_global/p_goal_y", p_goal(1));
    nh.getParam("/planning_return_global/p_goal_z", p_goal(2));
    nh.getParam("/planning_return_global/MAX_V_XY", MAX_V_XY);
    nh.getParam("/planning_return_global/MAX_V_Z_UP", MAX_V_Z_UP);
    nh.getParam("/planning_return_global/MAX_V_Z_DOWN", MAX_V_Z_DOWN);
    nh.getParam("/planning_return_global/MAX_A", MAX_A);

    nh.getParam("/planning_return_global/trajectory_1_send_counter", trajectory_1_send_counter);
    nh.getParam("/planning_return_global/ABSOLUTE_SAFE_TIME_PIECE1", ABSOLUTE_SAFE_TIME_PIECE1);
    nh.getParam("/planning_return_global/ABSOLUTE_SAFE_TIME_PIECE2", ABSOLUTE_SAFE_TIME_PIECE2);

    nh.getParam("/planning_return_global/d_ref", pp.d_ref);
    nh.getParam("/planning_return_global/k1_xy", pp.k1_xy);
    nh.getParam("/planning_return_global/k1_z", pp.k1_z);
    nh.getParam("/planning_return_global/k2_xy", pp.k2_xy);
    nh.getParam("/planning_return_global/k2_z", pp.k2_z);
    nh.getParam("/planning_return_global/v_max_at_goal_ori", pp.v_max_at_goal_ori);
    nh.getParam("/planning_return_global/collision_threshold_static", pp.collision_threshold_static);
    nh.getParam("/planning_return_global/risk_checking_radius", pp.risk_checking_radius);
    nh.getParam("/planning_return_global/dynamic_future_risk_threshold", pp.dynamic_future_risk_threshold);
    nh.getParam("collision_threshold_dynamic", pp.collision_threshold_dynamic);

    nh.getParam("/planning_return_global/if_plan_vertical_path", if_plan_vertical_path);
    nh.getParam("/planning_return_global/if_in_simulation", if_in_simulation);
    nh.getParam("/planning_return_global/if_flyback_enable", if_flyback_enable);
    nh.getParam("if_use_fence", if_use_fence);

    nh.getParam("/planning_return_global/HEIGHT_LIMIT", HEIGHT_LIMIT);
    nh.getParam("/planning_return_global/XY_LIMIT", XY_LIMIT);
    nh.getParam("/planning_return_global/PLAN_INIT_STATE_CHANGE_THRESHOLD", PLAN_INIT_STATE_CHANGE_THRESHOLD);
    nh.getParam("/planning_return_global/DIRECTION_CHANGE_LEFT_SIZE", DIRECTION_CHANGE_LEFT_SIZE);
    nh.getParam("/planning_return_global/DIRECTION_AUTO_CHANGE", DIRECTION_AUTO_CHANGE);
    nh.getParam("/planning_return_global/MAP_DELAY_SECONDS", MAP_DELAY_SECONDS);

    nh.getParam("/planning_return_global/TABLE_PATH", TABLE_PATH);
    nh.getParam("/planning_return_global/GOLBAL_TRAJECTORY_PATH", GOLBAL_TRAJECTORY_PATH);
    nh.getParam("/planning_return_global/connect_global_path_threshold", pp.connect_global_path_threshold);
    nh.getParam("/planning_return_global/k_near_global_traj", pp.k_near_global_traj);
    nh.getParam("/planning_return_global/k_long_dist_ref", pp.k_long_dist_ref);
    nh.getParam("/planning_return_global/traj1_points_num_to_keep_global", traj1_points_num_to_keep_global);

    p_goal_raw = p_goal;
}


int main(int argc, char** argv) {

    ros::init(argc, argv, "planning_test");
    ros::NodeHandle nh;


    setParameters();
    getParameterList(nh); /// chg, use parameters defined in the yaml file
    ROS_WARN("MAX_V=%f, MAX_A=%f, V_final_ref=%f, if_plan_vertical_path=%d, if_flyback_enable=%d", MAX_V_XY, MAX_A, pp.v_max_at_goal_ori, if_plan_vertical_path, if_flyback_enable);

    /*** Map initialization ***/
    my_map.setPredictionVariance(0.1, 0.1);  //0.1, 0.1
    my_map.setObservationStdDev(0.2); //0.02
//    my_map.setParticleRecordFlag(1, 22.5);
    my_map.setNewBornParticleNumberofEachPoint(25); //dynamic: 25  //static: 8
    my_map.setNewBornParticleWeight(0.0001); //0.01
    VideMap::setOriginalVoxelFilterResolution(res);


    /*** Read table ***/
    trajectory_time_table->csv2pva_table(TABLE_PATH);
    ROS_WARN("Trajectory_time_table loaded!");

    /*** Read global trajectory ***/
    vector<vector<double>> global_trajectory;
    if(readCSVTraj(GOLBAL_TRAJECTORY_PATH, global_trajectory)){
        /// Decode, 0-2: p, 3-5: v, 6-8: a
        for(auto & i : global_trajectory)
        {
            Eigen::Vector3d p_sp, v_sp, a_sp;
            p_sp << i[0], i[1], i[2];
            v_sp << i[3], i[4], i[5];
            a_sp << i[6], i[7], i[8];
            global_traj_p.push_back(p_sp);
            global_traj_v.push_back(v_sp);
            global_traj_a.push_back(a_sp);
        }

        global_trajectory_imported = true;
        ROS_WARN("Global trajectory loaded! rows = %ld", global_traj_p.size());
    }else{
        ROS_WARN("No global trajectory. Will fly to the goal position with local planning.");
    }


    /// Set key points
    if(global_trajectory_imported){
        setGlobalTrajectoryKeyPoints();
        send_global_path = true;
    }

    /*** Set planning parameters ***/
    p0 << 0.0, 0.0, 0.0;
    v0 << 0.0, 0.0, 0.0;
    a0 << 0.0, 0.0, 0.0;
//    p_goal << -1.0, -5.0, 1.2;
    yaw0 = 0.0;

    // Fov sample parameters
    Fov_half << 42, 27;

    if(if_flyback_enable){
        Angle_h_dense<< -130, -90, -75, -60, -45, -30, -25, -20, -15, -10, -5, 0, 5, 10, 15, 20, 25, 30, 45, 60, 75, 90, 130;
        Angle_h_sparse << -120, -100, -75, -50, -25, 0, 25, 50, 75, 100, 120;
    }else{
        Angle_h_dense << -90, -80, -70, -60, -50, -40, -30, -20, -10, -5, 0, 5, 10, 20, 30, 40, 50, 60, 70, 80, 90;
        Angle_h_sparse << -75, -60, -45, -30, -10, 0, 10, 30, 45, 60, 75;
    }

    if(if_plan_vertical_path){
        Angle_v_dense << -60, -50, -40, -30, -20, -10, 0, 10, 20, 30, 40, 50, 60;
        Angle_v_sparse << -30, -16, 0, 16, 30;
    }

    if(Angle_h_sparse.size() != ANGLE_H_NUM_SPARSE || Angle_v_sparse.size() != ANGLE_V_NUM_SPARSE){
        ROS_INFO(" ANGLE_H_NUM_SPARSE or ANGLE_V_NUM_SPARSE mismatches the initialized vector. Please check!");
        return 0;
    }

    Angle_h_sparse = Angle_h_sparse * M_PI / 180.0; // degree to rad
    Angle_v_sparse = Angle_v_sparse * M_PI / 180.0;
    Angle_h_dense = Angle_h_dense * M_PI / 180.0; // degree to rad
    Angle_v_dense = Angle_v_dense * M_PI / 180.0;

    Fov_half = Fov_half * M_PI / 180.0;

    if(if_in_simulation){offboard_ready=true;}

    ros::Duration(2).sleep();


    /** Define message handlers **/
    ros::Subscriber mode_sub = nh.subscribe("/mavros/state", 1, uavModeCallback);

    pva_pub = nh.advertise<trajectory_msgs::JointTrajectoryPoint>("/pva_setpoint", 1, true);
    current_marker_pub = nh.advertise<visualization_msgs::Marker>("/visualization_marker", 1);

    ros::Subscriber position_isolate_sub =  nh.subscribe("/vrpn_client_node/drone/pose", 1, positionCallback);
    ros::Subscriber velocity_isolate_sub = nh.subscribe("/vrpn_client_node/drone/twist", 1, velocityCallback);

    /// Obstacles
    ros::Subscriber position_box_pose_sub = nh.subscribe("/vrpn_client_node/box/pose", 1, boxPoseCallback);
    ros::Subscriber position_purple_pose_sub = nh.subscribe("/vrpn_client_node/purple/pose", 1, purplePoseCallback);

    ros::Subscriber position_four_wheel_pose_sub = nh.subscribe("/vrpn_client_node/four_wheel/pose", 1, fourWheelPoseCallback);
    ros::Subscriber position_four_wheel_vel_sub = nh.subscribe("/vrpn_client_node/four_wheel/twist", 1, fourWheelVelCallback);
    ros::Subscriber position_three_wheel_pose_sub = nh.subscribe("/vrpn_client_node/three_wheel/pose", 1, threeWheelPoseCallback);
    ros::Subscriber position_three_wheel_vel_sub = nh.subscribe("/vrpn_client_node/three_wheel/twist", 1, threeWheelVelCallback);


    ros::Timer timer1 = nh.createTimer(ros::Duration(0.1), trajectoryCallback);
    ros::Timer timer2 = nh.createTimer(ros::Duration(CAL_DURATION), sendCallback);


//    ros::spin();
    ros::AsyncSpinner spinner(4); // Use 4 threads
    spinner.start();
    ros::waitForShutdown();

    return 0;
}


void setParameters(){
    p_goal(0) = 60.0;
    p_goal(1)= 0.0;
    p_goal(2)=1.0;
    p_goal_raw = p_goal;

    MAX_V_XY = 3;
    MAX_V_Z_UP = 3;
    MAX_V_Z_DOWN = 3;
    MAX_A = 4;
    trajectory_1_send_counter = 1;  // To eliminate the influence of delay
    ABSOLUTE_SAFE_TIME_PIECE1 = 1.f;
    ABSOLUTE_SAFE_TIME_PIECE2 = 1.f;
    pp.d_ref = 2.0;
    pp.k1_xy = 8.0;
    pp.k1_z = 8.0;
    pp.k2_xy = 4.0;
    pp.k2_z = 4.0;
    pp.v_max_at_goal_ori = 0.5;
    pp.collision_threshold_static = 0.2;
    pp.collision_threshold_dynamic = 0.8;
    pp.risk_checking_radius = 0.5;
    pp.dynamic_future_risk_threshold = 40.0;
    pp.k_near_global_traj = 2.0;
    pp.k_long_dist_ref = 3.0;
    pp.connect_global_path_threshold = 0.5;
    traj1_points_num_to_keep_global = 40;

    if_plan_vertical_path = false;
    if_in_simulation = true;
    if_flyback_enable = false;
    if_use_fence = true;

    HEIGHT_LIMIT = 2.5;
    XY_LIMIT = 2.2;
    PLAN_INIT_STATE_CHANGE_THRESHOLD = 0.1;
    DIRECTION_CHANGE_LEFT_SIZE = 1.5;
    DIRECTION_AUTO_CHANGE = false;
    MAP_DELAY_SECONDS = 0.05;
    TABLE_PATH ="/home/clarence/ros_ws/map_ws/src/tables/p3-84_v3_a4_res0-1.csv";
//    GOLBAL_TRAJECTORY_PATH = "/home/clarence/ros_ws/map_ws/src/dynamic_map_planner/cfg/global_traj.csv";
}
